#include "CBeam.h"

CBeam::CBeam(CNode *n1, CNode *n2)
{
    if (n1->getId()<n2->getId())
    {
        nodes[0]=n1;
        nodes[1]=n2;
    }
    else
    {
        nodes[0]=n2;
        nodes[1]=n1;
    }
}

CBeam::~CBeam()
{
    //dtor
}

void CBeam::setProperties(double vE, double vA, double vJY, double vJZ, double vGJ)
{
    E=vE;
    A=vA;
    JY=vJY;
    JZ=vJZ;
    GJ=vGJ;
    length=nodes[0]->getDistance(nodes[1]);
}

void CBeam::createStiffnessMatrix()
{
    int i,j,k;
    if (K!=NULL)
        delete K;
    K=new CMatrix(12,12);
    CMatrix *t=new CMatrix(12,12);
    CMatrix *tt=new CMatrix(12,12);
    CMatrix *cc=new CMatrix(12,12);
    CMatrix *r0=new CMatrix(3,3);
    double ea=E*A;
    double eiy=E*JY;
    double eiz=E*JZ;
    double gj=GJ;
    double x1=nodes[0]->getCoord(X);
    double y1=nodes[0]->getCoord(Y);
    double z1=nodes[0]->getCoord(Z);
    double x2=nodes[1]->getCoord(X);
    double y2=nodes[1]->getCoord(Y);
    double z2=nodes[1]->getCoord(Z);
    x1=x2-x1;
    y1=y2-y1;
    z1=z2-z1;
    double ell=sqrt(x1*x1+y1*y1+z1*z1);
    double a1=ea/ell;
    double a2=12.*eiz/(ell*ell*ell);
    double a3=12.*eiy/(ell*ell*ell);
    double a4=6.*eiz/(ell*ell);
    double a5=6.*eiy/(ell*ell);
    double a6=4.*eiz/ell;
    double a7=4.*eiy/ell;
    double a8=gj/ell;
    K->setP1(1,1,a1);
    K->setP1(7,7,a1);
    K->setP1(1,7,-a1);
    K->setP1(7,1,-a1);
    K->setP1(2,2,a2);
    K->setP1(8,8,a2);
    K->setP1(2,8,-a2);
    K->setP1(8,2,-a2);
    K->setP1(3,3,a3);
    K->setP1(9,9,a3);
    K->setP1(3,9,-a3);
    K->setP1(9,3,-a3);
    K->setP1(4,4,a8);
    K->setP1(10,10,a8);
    K->setP1(4,10,-a8);
    K->setP1(10,4,-a8);
    K->setP1(5,5,a7);
    K->setP1(11,11,a7);
    K->setP1(5,11,0.5*a7);
    K->setP1(11,5,0.5*a7);
    K->setP1(6,6,a6);
    K->setP1(12,12,a6);
    K->setP1(6,12,0.5*a6);
    K->setP1(12,6,0.5*a6);
    K->setP1(2,6,a4);
    K->setP1(6,2,a4);
    K->setP1(2,12,a4);
    K->setP1(12,2,a4);
    K->setP1(6,8,-a4);
    K->setP1(8,6,-a4);
    K->setP1(8,12,-a4);
    K->setP1(12,8,-a4);
    K->setP1(5,9,a5);
    K->setP1(9,5,a5);
    K->setP1(9,11,a5);
    K->setP1(11,9,a5);
    K->setP1(3,5,-a5);
    K->setP1(5,3,-a5);
    K->setP1(3,11,-a5);
    K->setP1(11,3,-a5);
    double pi=3.141592654;
    double gamma=0.;
    double cg=cos(gamma);
    double sg=sin(gamma);
    double den=ell*sqrt(x1*x1+z1*z1);
    if (den!=0.)
    {
        r0->setP1(1,1,x1/ell);
        r0->setP1(1,2,y1/ell);
        r0->setP1(1,3,z1/ell);
        r0->setP1(2,1,(-x1*y1*cg-ell*z1*sg)/den);
        r0->setP1(2,2,den*cg/(ell*ell));
        r0->setP1(2,3,(-y1*z1*cg+ell*x1*sg)/den);
        r0->setP1(3,1,(x1*y1*sg-ell*z1*cg)/den);
        r0->setP1(3,2,-den*sg/(ell*ell));
        r0->setP1(3,3,(y1*z1*sg+ell*x1*cg)/den);
    }
    else
    {
        r0->setP1(1,1,0.);
        r0->setP1(1,3,0.);
        r0->setP1(2,2,0.);
        r0->setP1(3,2,0.);
        r0->setP1(1,2,1.);
        r0->setP1(2,1,-cg);
        r0->setP1(3,3,cg);
        r0->setP1(2,3,sg);
        r0->setP1(3,1,sg);
    }
    double x;
    for (i=1;i<=3;i++)
        for (j=1;j<=3;j++)
        {
            x=r0->getP1(i,j);
            for (k=0;k<=9;k+=3)
            {
                t->setP1(i+k,j+k,x);
                tt->setP1(j+k,i+k,x);
            }
        }
    for (i=1;i<=12;i++)
        for (j=1;j<=12;j++)
        {
            double sum=0.;
            for (k=1;k<=12;k++)
            {
                sum+=K->getP1(i,k)*t->getP1(k,j);
            }
            cc->setP1(i,j,sum);
        }
    for (i=1;i<=12;i++)
        for (j=1;j<=12;j++)
        {
            double sum=0.;
            for (k=1;k<=12;k++)
            {
                sum+=tt->getP1(i,k)*cc->getP1(k,j);
            }
            K->setP1(i,j,sum);
        }
    //K->print();
}

CMatrix* CBeam::getStiffnessMatrix()
{
    return K;
}

CNode* CBeam::getNode(int n)
{
    return nodes[n];
}
